#! /usr/bin/env python

# Copyright (c) 2010, Arizona Robotics Research Group, University of Arizona
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# 
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Willow Garage, Inc. nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS'
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Matt Brockman

PKG = 'test_manipulation'
NAME = 'line_of_sight_check'
ANGLE_CONSTRAINT_RIGHT = 1.2 #Arm can't move more than 70 Degrees to right
ANGLE_CONSTRAINT_LEFT = -1.2 #Arm can't move more than 70 Degrees to left

import roslib; roslib.load_manifest(PKG)
import rospy
import tf
import math

from cs665_manipulation_srvs.srv import *
from geometry_msgs.msg import PointStamped


#figures out if a coordinate is in line of sight of the arm (well, LOS ignores intervening objects)
class LineOfSightCheck():

    def __init__(self):

        rospy.init_node(NAME + 'server' , anonymous=True)
        self.tf = tf.TransformListener()

        self.service = rospy.Service(NAME, LOSService, self.handle_point)
        rospy.loginfo("%s: Ready for pointing", NAME)

    #handles receiving a PointStamped.  Returns target_in_sight
    def handle_point(self, req):
        rospy.loginfo("%s: Received position %s %s %s in %s", \
                        NAME, req.position.point.x, req.position.point.y, req.position.point.z, req.position.header.frame_id);
        return self.target_in_sight(req.position.header.frame_id, req.position.point.x, req.position.point.y, req.position.point.z)

    def find_point_angles(self, point):

        # arm_pan_link is situated in arm_base_link
        target_frame = 'arm_base_link'

        # Wait for tf info (time-out in 5 seconds)
        self.tf.waitForTransform(target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))

        # Transform target point & retrieve the pan angle
        pan_target = self.tf.transformPoint(target_frame, point)
        pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)

        return pan_angle

    # Figure out if something is in front of us (using only angles of view, ANGLE_CONSTRAINT_RIGHT and ANGLE_CONSTRAINT_LEFT)
    # if target is to left of current direction, returns -1, if to right, returns 1, if in view, returns 0
    def target_in_sight(self, frame_id, x, y, z):
        
        goal = SmartArmGoal()
        goal.target_point = PointStamped()
        goal.target_point.header.frame_id = frame_id
        goal.target_point.point.x = x
        goal.target_point.point.y = y
        goal.target_point.point.z = z

        #grab the pan angle (the first item in the list that find_point_angles returns)
        pan_angle = self.find_point_angles(goal.target_point)

        if (pan_angle > ANGLE_CONSTRAINT_RIGHT): #object to right of LOS field
            return 1
        elif (pan_angle < ANGLE_CONSTRAINT_LEFT): #object to left of LOS field
            return -1        
        return 0 #object in LOS field

if __name__ == '__main__':
    try:
        pointle = LineOfSightCheck()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

